#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>
#include <string.h>
#include <sys/mman.h>
#include <assert.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>

using namespace std;
using namespace cv;

#define  IMAGE_W 800
#define  IMAGE_H 1067

//#define  IMAGE_W 1296
//#define  IMAGE_H 976

#define  FILE_NAME "output422.yuv"

int convert_yuv_to_rgb_pixel(int y, int u, int v) {
	unsigned int pixel32 = 0;
	unsigned char *pixel = (unsigned char*) &pixel32;
	int r, g, b;
	r = y + (1.370705 * (v - 128));
	g = y - (0.698001 * (v - 128)) - (0.337633 * (u - 128));
	b = y + (1.732446 * (u - 128));

	if (r > 255)
		r = 255;
	if (g > 255)
		g = 255;
	if (b > 255)
		b = 255;
	if (r < 0)
		r = 0;
	if (g < 0)
		g = 0;
	if (b < 0)
		b = 0;

	pixel[0] = b;  // blue
	pixel[1] = g;  // green
	pixel[2] = r;  //red
	return pixel32;
}

int convert_yuv_to_rgb_buffer(unsigned char *yuv, unsigned char *rgb,
		unsigned int width, unsigned int height) {
	unsigned int in = 0;
	unsigned int pixel32;
	unsigned char *ptr = rgb;
	int y0, u, y1, v;
	while (in < width * height * 2) {
		y0 = yuv[in++];
		u = yuv[in++];
		y1 = yuv[in++];
		v = yuv[in++];
		pixel32 = convert_yuv_to_rgb_pixel(y0, u, v);
		*(ptr++) = (pixel32 & 0x000000ff);
		*(ptr++) = (pixel32 & 0x0000ff00) >> 8;
		*(ptr++) = (pixel32 & 0x00ff0000) >> 16;
		pixel32 = convert_yuv_to_rgb_pixel(y1, u, v);
		*(ptr++) = (pixel32 & 0x000000ff);
		*(ptr++) = (pixel32 & 0x0000ff00) >> 8;
		*(ptr++) = (pixel32 & 0x00ff0000) >> 16;
	}
	return 0;
}

/**
 * yuyv422   to  rgb  to  jpg.
 *   ffmpeg -i in.jpg -pix_fmt yuyv422 output422.yuv -y
 *   ffmpeg  -pix_fmts
 *   yuyv422 use 4bytes to  store  2 pixes.
 *   y1,Cr,y2,Cb.
 *   first pixel : y1,Cr,Cb.
 *   second :      y2,Cr,Cb.
 */
int main() {
	int ret;
	int fd = open(FILE_NAME, O_RDONLY);
	unsigned char yuv[IMAGE_W * IMAGE_H * 2 + 1000];
	cout << "0" << endl;
	ret = read(fd, yuv, IMAGE_W * IMAGE_H * 2 + 1000);
	if (ret == -1) {
		perror("read");
		return -1;
	}
	assert(ret== IMAGE_W * IMAGE_H * 2);
	char rgb24[IMAGE_W * IMAGE_H * 3];
	convert_yuv_to_rgb_buffer((unsigned char*) yuv, (unsigned char*) rgb24,
	IMAGE_W, IMAGE_H);
	Mat image(IMAGE_H, IMAGE_W, CV_8UC3, rgb24);
	imwrite("output.jpg", image);
	cout << "!!!Hello World!!01220!" << endl; // prints !!!Hello World!!!
	return 0;
}
